/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file    can.c
 * @brief   This file provides code for the configuration
 *          of the CAN instances.
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "pro_can.h"
#include "motor_control.h"
#include "control_config.h"
#include "loop.h"
#include "signal_port.h"
#include "Current_Tracker.h"
#include "Location_Tracker.h"
#include "Speed_Tracker.h"
#include "mt6816.h"
#include "hw_elec.h"
/* USER CODE BEGIN 0 */
#if CAN_RX0_INT_ENABLE /* 使能RX0中断 */

#define MAX_Current 2500

/* 用查找表或函数指针数组优化命令分发。方便协议扩展 */
typedef void (*CommandHandler)(const uint8_t* data, uint8_t len);
static uint8_t cmd_data[16] = {0};
static uint16_t crc_result = 0;
static __IO Goal_Param temp = {0};


typedef struct {
    uint32_t cmd_id;          // 命令标识符
    CommandHandler handler;  // 处理函数指针
} CommandEntry;

// 静态数组
#define MAX_COMMANDS 0x30                           // 注册命令
static CommandEntry command_table[MAX_COMMANDS];    // 命令表
static uint32_t command_count = 0;
static ErrStatus flag = ERROR;

/* 命令注册函数 */
int register_command(uint32_t cmd_id, CommandHandler handler) {
    if (command_count >= MAX_COMMANDS) {
        return -1; // 表满，注册失败
    }
    command_table[command_count].cmd_id = cmd_id;
    command_table[command_count].handler = handler;
    command_count++;
    return 0;
}

/**
 * @brief 检查CAN接收的模式参数是否合法
 * @param mode 接收到的模式参数（uint8_t类型）
 * @return true-合法模式，false-非法模式
 */
bool IsValidMotorMode(uint8_t mode) 
{
    // 根据实际 Motor_Mode 枚举定义调整校验条件
    switch(mode) {
        case Control_Mode_Stop:
        case Motor_Mode_Digital_Location:
        case Motor_Mode_Digital_Speed:
        case Motor_Mode_Digital_Current:
        case Motor_Mode_Digital_Track:
        case Motor_Mode_PWM_Location:
        case Motor_Mode_PWM_Speed:
        case Motor_Mode_PWM_Current:
        case Motor_Mode_PULSE_Location:
            return true;
        default:
            return false;
    }
}

/* 定义具体命令处理函数 */
/* 读取电机ID */
static void read_motor_id(const uint8_t* data, uint8_t len) 
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_DEVICE_ID;
    cmd_data[2] = motor_control.device_id;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    can_send_msg(&cmd_data[0], 5);
}
/* 读取电机状态 */
static void read_motor_status(const uint8_t* data, uint8_t len) 
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_MOTOR_STATUS;
    cmd_data[2] = motor_control.state;                    // 运行状态
    cmd_data[3] = motor_control.stall_flag;               // 堵转标志
    cmd_data[4] = motor_control.overload_flag;            // 过载标志
    cmd_data[5] = motor_control.soft_disable;             // 软失能标志
    cmd_data[6] = mt6816.rectify_valid;                   // 校准有效标记
    crc_result = crc16_ccitt(&cmd_data[1], 6, 0xFFFF);
    cmd_data[7] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[8] = crc_result & 0xFF;        // 低字节;
    can_send_msg(&cmd_data[0], 9);
}
/* 读取FOC电流 */
static void read_foc_current(const uint8_t* data, uint8_t len)
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_FOC_CURRENT;
    cmd_data[2] = (uint8_t)(motor_control.foc_current >> 24);
    cmd_data[3] = (uint8_t)(motor_control.foc_current >> 16);
    cmd_data[4] = (uint8_t)(motor_control.foc_current >> 8);
    cmd_data[5] = (uint8_t)(motor_control.foc_current);
    crc_result = crc16_ccitt(&cmd_data[1], 5, 0xFFFF);
    cmd_data[6] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[7] = crc_result & 0xFF;        // 低字节;
    can_send_msg(&cmd_data[0], 8);
}
/* 读取foc位置 */
static void read_foc_location(const uint8_t* data, uint8_t len)
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_FOC_LOCATION;
    cmd_data[2] = (uint8_t)(motor_control.foc_location >> 24);
    cmd_data[3] = (uint8_t)(motor_control.foc_location >> 16);
    cmd_data[4] = (uint8_t)(motor_control.foc_location >> 8);
    cmd_data[5] = (uint8_t)(motor_control.foc_location);
    crc_result = crc16_ccitt(&cmd_data[1], 5, 0xFFFF);
    cmd_data[6] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[7] = crc_result & 0xFF;        // 低字节;
    can_send_msg(&cmd_data[0], 8);
}
/* 读取累计脉冲数（用于计算当前步数） */
static void read_pulse_count(const uint8_t* data, uint8_t len)
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_Pulse_NUM;
    cmd_data[2] = (uint8_t)(step_num >> 24);
    cmd_data[3] = (uint8_t)(step_num >> 16);
    cmd_data[4] = (uint8_t)(step_num >> 8);
    cmd_data[5] = (uint8_t)(step_num);
    crc_result = crc16_ccitt(&cmd_data[1], 5, 0xFFFF);
    cmd_data[6] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[7] = crc_result & 0xFF;        // 低字节;
    can_send_msg(&cmd_data[0], 8);
}
/* 读取额定电流 */
static void read_rated_current(const uint8_t* data, uint8_t len)
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_RCUR;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[0] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[0] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取校准电流 */
static void read_calibration_current(const uint8_t* data, uint8_t len)
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_CCUR;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[1] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[1] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取电机细分 */
static void read_motor_division(const uint8_t* data, uint8_t len)
{
    uint16_t crc_result = 0;
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_Divide;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[2] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[2] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取额定转速 */
static void read_rated_speed(const uint8_t* data, uint8_t len)
{
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_RATE_SPEED;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[3] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[3] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取固件额定加速加速度 */
static void read_rated_acceleration(const uint8_t* data, uint8_t len)
{
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_RATE_UPACC;
    cmd_data[2] = (uint8_t)((dec_param_reg[4] >> 8) & 0xFF); // 高8位
    cmd_data[3] = (uint8_t)(dec_param_reg[4] & 0xFF);        // 低8位
    cmd_data[4] = ReadSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取固件额定减速加速度 */
static void read_rated_deceleration(const uint8_t* data, uint8_t len)
{
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_RATE_DOWNACC;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[5] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[5] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取固件额定增流梯度 */
static void read_rated_up_current_rate(const uint8_t* data, uint8_t len)
{
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_RATE_UPCURRATE;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[6] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[6] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 读取固件额定减流梯度 */
static void read_rated_down_current_rate(const uint8_t* data, uint8_t len)
{
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = R_RATE_DOWNCURRATE;
    cmd_data[2] = ReadSuccess;
    cmd_data[3] = (uint8_t)((dec_param_reg[7] >> 8) & 0xFF); // 高8位
    cmd_data[4] = (uint8_t)(dec_param_reg[7] & 0xFF);        // 低8位
    crc_result = crc16_ccitt(&cmd_data[1], 4, 0xFFFF);
    cmd_data[5] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[6] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 7);
}
/* 设置额定电流 */
static void set_rated_current(const uint8_t* data, uint8_t len)
{
    flag = setCurrentRatedCurrent((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_RCUR;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF); // CRC校验
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置校准电流 */
static void set_calibration_current(const uint8_t* data, uint8_t len)
{
    flag = setCurrent_Cali_Current((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_CCUR;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置电机细分 */
static void set_motor_div(const uint8_t* data, uint8_t len)
{
    flag = setMoveDivideNum((data[1] << 8) + (data[2]));
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_Divide;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置额定转速 */
static void set_rated_speed(const uint8_t* data, uint8_t len)
{
    flag = setMoveRatedSpeed((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_RATE_SPEED;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置固件额定加速加速度 */
static void set_rate_acc(const uint8_t *data, uint8_t len) 
{
    flag = setMoveRatedUpAcc((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_RATE_UPACC;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置固件额定减速加速度 */
static void set_rate_down_acc(const uint8_t *data, uint8_t len) 
{
    flag = setMoveRatedDownAcc((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_RATE_DOWNACC;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置固件额定增流梯度 */
static void set_rate_up_rate(const uint8_t *data, uint8_t len)
{
    flag = setMoveRatedUpCurrentRate((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_RATE_UPCURRATE;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置固件额定减流梯度 */
static void set_rate_down_rate(const uint8_t *data, uint8_t len)
{
    flag = setMoveRatedDownCurrentRate((bsp_can.CanRx.rx_data[1] << 8) + (bsp_can.CanRx.rx_data[2]));
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_RATE_DOWNCURRATE;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置电机运行模式 */
static void set_motor_run_mode(const uint8_t *data, uint8_t len)
{
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MODE;
    if (IsValidMotorMode(data[1]))
    {
        Motor_Control_SetMotorMode((Motor_Mode)data[1]);
        cmd_data[2] = SetSuccess;
    }
    else
        cmd_data[2] = Illegal_Mode;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置DCE参数 */
static void set_dce_param(const uint8_t* data, uint8_t len)
{
    switch (data[1])
    {
        case DCE_KP:
            flag = Control_DCE_SetKP((data[2] << 8) + (data[3]));
            cmd_data[2] = DCE_KP;
            break;
        case DCE_KI:
            flag = Control_DCE_SetKI((data[2] << 8) + (data[3]));
            cmd_data[2] = DCE_KI;
            break;
        case DCE_KV:
            flag = Control_DCE_SetKV((data[2] << 8) + (data[3]));
            cmd_data[2] = DCE_KV;
            break;
        case DCE_KD:
            flag = Control_DCE_SetKD((data[2] << 8) + (data[3]));
            cmd_data[2] = DCE_KD;
            break;
        default:
            break;
    }
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_DCE;
    cmd_data[3] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 3, 0xFFFF);
    cmd_data[4] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[5] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 6);
}
/* 设置PID参数 */
static void set_pid_param(const uint8_t* data, uint8_t len)
{
    switch (data[1])
    {
        case PID_KP:
            flag = Control_PID_SetKP((data[2] << 8) + (data[3]));
            cmd_data[2] = PID_KP;
            break;
        case PID_KI:
            flag = Control_PID_SetKI((data[2] << 8) + (data[3]));
            cmd_data[2] = PID_KI;
            break;
        case PID_KD:
            flag = Control_PID_SetKD((data[2] << 8) + (data[3]));
            cmd_data[2] = PID_KD;
            break;
        default:
            break;
    }
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_PID;
    cmd_data[3] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 3, 0xFFFF);
    cmd_data[4] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[5] = crc_result & 0xFF;        // 低字节;
    can_send_msg(&cmd_data[0], 6);
}
/* 设置硬目标位置 */
static void set_goal_location(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    Motor_Control_Write_Goal_Location(temp.val);
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_GOAL_LOACTION;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置硬目标速度*/
static void set_goal_speed(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    Motor_Control_Write_Goal_Speed(temp.val);
    /*打包回复信息*/
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_GOAL_SPEED;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /*回复*/
    can_send_msg(&cmd_data[0], 5);
}
/* 设置硬目标电流 */
static void set_goal_current(const uint8_t* data, uint8_t len)
{
    /* 设置硬目标电流 */
    temp.val = 0;
    temp.bytes[1] = (bsp_can.CanRx.rx_data[1]);
    temp.bytes[0] = (bsp_can.CanRx.rx_data[2]);
    Motor_Control_Write_Goal_Current(temp.val16[0]);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_GOAL_CURRENT;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置软目标位置加速度*/
static void set_location_up_acc(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Location_Tracker_Set_UpAcc(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_LOACTION_UP_ACC;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置软目标速度加速度 */
static void set_speed_up_acc(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Speed_Tracker_Set_UpAcc(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_SPEED_UP_ACC;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置软目标电流增流梯度 */
static void set_current_up_rate(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Current_Tracker_Set_UpRate(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_CURRENT_UP_RATE;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置软目标位置减速度 */
static void set_location_down_acc(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Location_Tracker_Set_DownAcc(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_LOACTION_DOWN_ACC;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置软目标速度减速度 */
static void set_speed_down_acc(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Speed_Tracker_Set_DownAcc(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_SPEED_DOWN_ACC;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置软目标电流减流梯度 */
static void set_current_down_rate(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Current_Tracker_Set_DownRate(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_CURRENT_DOWN_RATE;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置位置模式最大速度 */
static void set_location_mode_max_speed(const uint8_t* data, uint8_t len)
{
    temp.val = 0;
    temp.bytes[3] = (data[1]);
    temp.bytes[2] = (data[2]);
    temp.bytes[1] = (data[3]);
    temp.bytes[0] = (data[4]);
    flag = Location_Tracker_Set_MaxSpeed(temp.val);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_LOCATION_MAX_SPEED;
    cmd_data[2] = (flag == SUCCESS ? SetSuccess : Illegal_Parameter);
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置堵转 */
static void set_motor_stall(const uint8_t* data, uint8_t len)
{
    /* 设置电机堵转保护开关 */
    Motor_Control_SetStallSwitch(data[1]);
    /* 设置电机堵转增流开关 */
    Motor_Control_SetStallUpRateSwitch(data[2]);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MOTOR_STALL;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 清除电机堵转标记*/
static void clear_motor_stall(const uint8_t* data, uint8_t len)
{
    Motor_Control_Clear_Stall();
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MOTOR_CLEAN_STALL;
    cmd_data[2] = Task_Success;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 电机参数保存 */
static void save_motor_param(const uint8_t* data, uint8_t len)
{
    flag = Config_Param_save();
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MOTOR_PARAM_SAVE;
    cmd_data[2] = (flag == SUCCESS? SaveSuccess : SaveFailure);
    crc_result = crc16_ccitt(&cmd_data[1], 5, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置设备ID */
static void set_device_id(const uint8_t* data, uint8_t len)
{
    /* 设置电机设备ID */
    Motor_Device_Id_Set(data[1]);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MOTOR_DEVICE_ID;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 电机停止运行 */
static void stop_motor(const uint8_t* data, uint8_t len)
{
    /* 电机停止运行 */
    if (motor_control.mode_run == Motor_Mode_Digital_Location)
    {
        motor_control.goal_speed = 0;

        // // 输出归零
        // motor_control.foc_current = 0;
        // motor_control.foc_location = 0;

        // 清除积分
        // Motor_Control_Clear_Integral();

        // 让驱动休眠
        REIN_HW_Elec_SetBrake();
    }
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MOTOR_STOP;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}
/* 设置多机同步 */
static void set_motor_synch(const uint8_t* data, uint8_t len)
{
    // /* 设置多机同步标记 */

    // /* 打包回复信息 */
    // cmd_data[0] = CAN_MASTERR_ADD;
    // cmd_data[1] = S_MOTOR_SYNCH;
    // cmd_data[2] = SetSuccess;
    // crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    // cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    // cmd_data[4] = crc_result & 0xFF;        // 低字节;
    // /* 回复 */
    // can_send_msg(&cmd_data[0], 5);
}
/* 设置零点 */
static void set_zero_location(const uint8_t* data, uint8_t len)
{
    /* 设置零点 */
    Motor_Zero_Set();
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_ZERO_LOCATION;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}

void set_motor_brake(const uint8_t* data, uint8_t len)
{
    /* 设置刹车 */
    Motor_Control_Write_Goal_Brake(data[1]);
    /* 打包回复信息 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = S_MOTOR_BRAKE;
    cmd_data[2] = SetSuccess;
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}

void default_handler(const uint8_t* data, uint8_t len) 
{
    /* 无效功能码 */
    cmd_data[0] = CAN_MASTERR_ADD;
    cmd_data[1] = data[0];    // 返回功能码
    cmd_data[2] = Illegal_Command;             // 返回错误码
    crc_result = crc16_ccitt(&cmd_data[1], 2, 0xFFFF);
    cmd_data[3] = (crc_result >> 8) & 0xFF; // 高字节
    cmd_data[4] = crc_result & 0xFF;        // 低字节;
    /* 回复 */
    can_send_msg(&cmd_data[0], 5);
}

/* 命令分发逻辑 */
void dispatch_command(const uint8_t* data, uint8_t len) 
{
    if (command_table[data[0]].cmd_id == data[0])
    {
        command_table[data[0]].handler(data, len);// 调用处理函数
        return;
    }
    // 未找到命令时的默认处理
    default_handler(data, len);
}

/* 初始化注册命令*/
void Pro_CAN_Init(void) 
{
    // CAN通信初始化
    BSP_CAN_Init();             

    // 协议注册
    register_command(R_DEVICE_ID,read_motor_id);
    register_command(R_MOTOR_STATUS, read_motor_status);
    register_command(R_FOC_CURRENT, read_foc_current);
    register_command(R_FOC_LOCATION, read_foc_location);
    register_command(R_Pulse_NUM, read_pulse_count);
    register_command(R_RCUR, read_rated_current);
    register_command(R_CCUR, read_calibration_current);
    register_command(R_Divide, read_motor_division);
    register_command(R_RATE_SPEED, read_rated_speed);
    register_command(R_RATE_UPACC, read_rated_acceleration);
    register_command(R_RATE_DOWNACC, read_rated_deceleration);
    register_command(R_RATE_UPCURRATE, read_rated_up_current_rate);
    register_command(R_RATE_DOWNCURRATE, read_rated_down_current_rate);
    register_command(S_RCUR, set_rated_current);
    register_command(S_CCUR, set_calibration_current);
    register_command(S_Divide, set_motor_div);
    register_command(S_RATE_SPEED, set_rated_speed);
    register_command(S_RATE_UPACC, set_rate_acc);
    register_command(S_RATE_DOWNACC, set_rate_down_acc);
    register_command(S_RATE_UPCURRATE, set_rate_up_rate);
    register_command(S_RATE_DOWNCURRATE, set_rate_down_rate);
    register_command(S_MODE, set_motor_run_mode);
    register_command(S_DCE, set_dce_param);
    register_command(S_PID, set_pid_param);
    register_command(S_GOAL_LOACTION, set_goal_location);
    register_command(S_GOAL_SPEED, set_goal_speed);
    register_command(S_GOAL_CURRENT, set_goal_current);
    register_command(S_LOACTION_UP_ACC, set_location_up_acc);
    register_command(S_SPEED_UP_ACC, set_speed_up_acc);
    register_command(S_CURRENT_UP_RATE, set_current_up_rate);
    register_command(S_LOACTION_DOWN_ACC, set_location_down_acc);
    register_command(S_SPEED_DOWN_ACC, set_speed_down_acc);
    register_command(S_CURRENT_DOWN_RATE, set_current_down_rate);
    register_command(S_LOCATION_MAX_SPEED, set_location_mode_max_speed);
    // register_command(S_MOTOR_SYNCH, set_motor_stall);
    register_command(S_MOTOR_STALL, set_motor_stall);
    register_command(S_MOTOR_CLEAN_STALL, clear_motor_stall);
    register_command(S_MOTOR_PARAM_SAVE, save_motor_param);
    register_command(S_MOTOR_DEVICE_ID, set_device_id);
    register_command(S_MOTOR_STOP, stop_motor);
    register_command(S_MOTOR_SYNCH, set_motor_synch);
    register_command(S_ZERO_LOCATION, set_zero_location);
    register_command(S_MOTOR_BRAKE, set_motor_brake);
    // 新增命令只需在此添加一行
}

/**
 * @brief       CAN RX0 中断服务函数
 *   @note      处理CAN FIFO0的接收中断
 * @param       无
 * @retval      无
 */
void USBD_LP_CAN0_RX0_IRQHandler()
{
    // /* 检查接收中断标志 */
    // if(can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) == RESET) {
    //     return;
    // }
    /* 接收报文 */
    can_message_receive(CAN0, CAN_FIFO0, &bsp_can.CanRx);
    // can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0);

    if (bsp_can.CanRx.rx_ff != CAN_FF_STANDARD || bsp_can.CanRx.rx_ft != CAN_FT_DATA) /* 不是标准帧 / 不是数据帧 */
    {
        return;
    }
    /* CRC校验 */
    // crc_result = crc16_ccitt(&bsp_can.CanRx.rx_data[0], bsp_can.CanRx.rx_dlen - 2, 0xFFFF);
    // if (crc_result != ((bsp_can.CanRx.rx_data[bsp_can.CanRx.rx_dlen - 2] << 8) | bsp_can.CanRx.rx_data[bsp_can.CanRx.rx_dlen - 1]))
    // {
    //     return; // CRC校验失败
    // }

    /* 分发命令 */
    dispatch_command(&bsp_can.CanRx.rx_data[0],bsp_can.CanRx.rx_dlen);
}

#endif

/* USER CODE END 0 */
